#include "TemperatureStuff.h"
#include "Serial.h"
#include "Config.h"
#include "MyCan.h"
#include "userConfig.h"
#include "CanMessage.h"
#include "leds.h"
#include <stdlib.h>
#include <string.h>
#include <limits.h>

#define INVALID_TEMPERATURE INT_MIN
#define MISSING_SENSOR INT_MIN + 1


#define TIMEOUTS_FOR_READING_TEMPERATURE 4
#define TIMEOUTS_BEFORE_ABANDON_EXTERNAL 20

static int HotCountDown;
#define HOT_COUNTDOWN_BASE 20

int ExternalDeviceCount;
ExternalSensorPacket ExternalData[MAX_EXTERNAL_DEVICES];

int TimerForReadingTemperatures;
static int ModuleAgedOut;
static int NewMessageRecieved;
static int OldCommandedState;

void hex_ascii(unsigned char);
void ResetExternalData(ExternalSensorPacket* data);
static void DetermineHeaterCommand();
static void SetCommandedState(int command);

void InitializeTemperatureStuff()
{
    int deviceCount;
    ExternalSensorPacket *data;
    for(deviceCount = 0; deviceCount < MAX_EXTERNAL_DEVICES; deviceCount++)
    {
        data = ExternalData + deviceCount;
        data->SA = 0xff;
        ResetExternalData(data);
        data->age = 0;
    }
    TimerForReadingTemperatures = 0;
    ModuleAgedOut = 0;
    NewMessageRecieved = 0;
    OW_PIN_DIRECTION = 0;
    SetCommandedState(0);
    HotCountDown = 0;
}

void ResetExternalData(ExternalSensorPacket* data)
{
    int byteIndex;
    for(byteIndex = 0; byteIndex < 7; byteIndex++)
    {
        data->tempValues[byteIndex] = 0xfe;
    }
}

void PrintAllTemperatures()
{
    int i;
    char buff[10];
    PutString("Commanded State: ");
    itoa(buff, OldCommandedState, 10);
    PutString(buff);
    Putchar('\r');
    for(i = 0; i < MAX_EXTERNAL_DEVICES; i++)
    {
        PutString("EM ");
        Putchar('0' + i);
        PutString(" SA:");
        hex_ascii(ExternalData[i].SA);
        PutString(" Data: ");
        int j;
        for(j = 0; j < 7; j++)
        {
            itoa(buff, ExternalData[i].tempValues[j], 10);
            PutString(buff);
            Putchar(' ');
        }

        PutString("Age: ");
        itoa(buff, ExternalData[i].age, 10);
        PutString(buff);
       Putchar('\r');
    }
}

unsigned char ConvertCelciusToCAN(int temperature)
{
    unsigned char retVal = 0xff;
    if(temperature != INVALID_TEMPERATURE)
    {
        if(temperature == MISSING_SENSOR)
        {
            retVal = 0xfe;
        }
        else
        {
        retVal = temperature + 100;
        }
    }
    return retVal;
}

void HandlerExternalTemperatureMessagePriv(unsigned char SA, unsigned char* data)
{
    int dataIndex;
    for(dataIndex = 0; dataIndex < ExternalDeviceCount; dataIndex++)
    {
        if(ExternalData[dataIndex].SA == SA)
        {
            break;
        }
    }

    if(dataIndex == ExternalDeviceCount)
    {
        ExternalDeviceCount++;
    }

    ExternalData[dataIndex].SA = SA;
    memcpy(ExternalData[dataIndex].tempValues, data + 1, 7);
    ExternalData[dataIndex].age = 0;
}

void TS_HandleExternalTemperatureMessageNew(CANMessage* message)
{
    NewMessageRecieved = 1;
    unsigned char SA = message->ID & 0xff;
    HandlerExternalTemperatureMessagePriv(SA, message->Data);
}


void TS_HandleTimerTick()
{
    TimerForReadingTemperatures++;
    int dataIndex;
    ExternalSensorPacket* data;
    for(dataIndex = 0; dataIndex < ExternalDeviceCount; dataIndex++)
    {
        data = ExternalData + dataIndex;
        if(data->SA != 0xFF)
        {
            data->age++;
            if(data->age > TIMEOUTS_BEFORE_ABANDON_EXTERNAL)
            {
                ModuleAgedOut = 1;
                ResetExternalData(data);
                data->age = TIMEOUTS_BEFORE_ABANDON_EXTERNAL;
            }
        }
    }
}

void TS_ProcessMainLoop()
{
    static int previousInternalButtonValue = 1;
    static int previousExternalButtonValue = 1;
    int shouldRun = 0;
    int timerExpired = 0;


    __builtin_disi(0x3FFF);
    shouldRun = ModuleAgedOut || NewMessageRecieved;
    ModuleAgedOut = 0;
    NewMessageRecieved = 0;
    if(TimerForReadingTemperatures > TIMEOUTS_FOR_READING_TEMPERATURE)
    {
        timerExpired = 1;
        TimerForReadingTemperatures = 0;
    }
    __builtin_disi(0);

    if(timerExpired)
    {
        LED_ToggleStatus();

       if(!PORTBbits.RB0 && !previousInternalButtonValue)
       {
           PutString("Syncing Node Count Nop\r");
       }
       previousInternalButtonValue = PORTBbits.RB0;

       if(!PORTBbits.RB1 && !previousExternalButtonValue)
       {
           PutString("Syncing External Node Count\r");
           SetExternalNodeCount(ExternalDeviceCount);
       }
       previousExternalButtonValue = PORTBbits.RB1;

    }

    if(shouldRun)
    {
        DetermineHeaterCommand();
        //figure out heater command
    }
}

void DetermineHeaterCommand()
{
    //if any module missing, kill output
    //if currently running, check for any too hot
    //if currently stopped, check for any too cold
    int anyModuleMissing = 0;
    int anyModuleTooHot = 0;
    int anyModuleTooCold = 0;
    int deviceIndex = 0;
    int sensorIndex = 0;

    int lowCANValue = ConvertCelciusToCAN(GetLowTemperatureThreshold());
    int highCANValue = ConvertCelciusToCAN(GetHighTemperatureThreshold());

    if(ExternalDeviceCount >= GetExternalNodeCount() && lowCANValue != highCANValue)
    {
        ExternalSensorPacket* device;
        for(deviceIndex = 0; deviceIndex < ExternalDeviceCount; deviceIndex++)
        {
            device = ExternalData + deviceIndex;
            for(sensorIndex = 0; sensorIndex < MAX_SENSORS_PER_DEVICE; sensorIndex++)
            {
                unsigned char temperatureValue = device->tempValues[sensorIndex];
                if(temperatureValue == 0xFE)
                {
                    anyModuleMissing = 1;
                    break;
                }
                if(temperatureValue != 0xFF)
                {
                    if(temperatureValue < lowCANValue)
                    {
                        anyModuleTooCold = 1;
                    }
                    if(temperatureValue > highCANValue)
                    {
                        anyModuleTooHot = 1;
                        HotCountDown = HOT_COUNTDOWN_BASE;
                        break;
                    }
                }
            }
        }
    }
    else
    {
        anyModuleMissing = 1;
    }

//    newState  AnyBad   OldState AnyCold
//       0        1           X        X
//       0        0           0        0
//       1        0           0        1
//       1        0           1        0
//       1        0           1        1
    if(HotCountDown)
    {
        HotCountDown--;
    }

    LED_1 = anyModuleMissing;
    LED_2 = anyModuleTooHot || HotCountDown;
    SetCommandedState(!(anyModuleMissing || anyModuleTooHot || HotCountDown) && (OldCommandedState || anyModuleTooCold));

}

void SetCommandedState(int state)
{
    OldCommandedState = state;
    OW_WRITE_PIN = state;
    LED_3 = state;
}
